#include "move_pose.h"


MovePosePlanner::MovePosePlanner(tf2_ros::Buffer* tf_buffer):LocalPlannerBase(tf_buffer){
    __kp_alpha = 3;
    __kp_beta = 1;
}


MovePosePlanner::~MovePosePlanner(){
    
}

// 更新缓存数据
void MovePosePlanner::UpdateCache(){

}

// 计算速度指令
// robot_pose  机器人当前位姿  map坐标系下
// goal_pose 目标点位姿  map坐标系下
bool MovePosePlanner::ComputeTwist(Velocity2D& cmd_vel, LidarScan2D lidar_scan_2d, 
    Pose2D robot_pose, Pose2D goal_pose){
    float dx = goal_pose.x - robot_pose.x;
    float dy = goal_pose.y - robot_pose.y;
    float dis = hypot(dx, dy);
    float ang = atan2(dy, dx);
    float alpha = math_utils::DifferenceAngle(ang, robot_pose.yaw);
    float beta = math_utils::DifferenceAngle(goal_pose.yaw, ang);

    // 机器人移动到目标点，先旋转alpha角度指向目标点，在移动dis距离，最后再旋转betha角度。
    // 权重先后算法函数
    // kp_alpha 机器人角度 与 机器人指向目标点角度 差
    // kp_betha 目标点角度 与 机器人指向目标点角度 差
    // 角速度评估函数 w = kp_alpha * alpha * dis + kp_beta * beta
    // kp_aplha、kp_beta为权重系数，kp_aplha > kp_beta  旋转alpha角度比旋转betha角度优先级更高，避免偏离目标点
    // 当离目标点dis越来越近时，第一项kp_alpha * alpha * dis影响越来越小
    cmd_vel.vy = 0.0;
    cmd_vel.vx = 0.3;
    cmd_vel.vth = __kp_alpha * alpha * dis + __kp_beta * beta;
}